#include "GQuaternion.h"
#include "GMatrix4x4.h"
#include "GMath.h"

namespace GMath
{

	const GQuaternion GQuaternion::Identitiy = GQuaternion(0.0f, 0.0f, 0.0f, 1.0f);

	GQuaternion::GQuaternion()
	{
		x = 0.0f;
		y = 0.0f; 
		z = 0.0f; 
		w = 1.0f;
	}

	GQuaternion::GQuaternion(const GVector3& Axis, float Angle)
	{
		float Cos = CosF(Angle / 2.0f);
		float Sin = SinF(Angle / 2.0f);
	
		x = Axis.x * Sin;
		y = Axis.y * Sin;
		z = Axis.z * Sin;
		w = Cos;

		Normalize();
	}

	GQuaternion::GQuaternion(float RotX, float RotY, float RotZ)
	{
		GQuaternion Qx(GVector3::UnitX, RotX);
		GQuaternion Qy(GVector3::UnitY, RotY);
		GQuaternion Qz(GVector3::UnitZ, RotZ);

		GQuaternion Qxy, Qxyz;
		GQuaternion::Multiply(Qxy, Qx, Qy);
		GQuaternion::Multiply(Qxyz, Qxy, Qz);

		x = Qxyz.x; 
		y = Qxyz.y; 
		z = Qxyz.z; 
		w = Qxyz.w;
	}

	GQuaternion::GQuaternion(float Qx, float Qy, float Qz, float Qw)
	{
		x = Qx;
		y = Qy;
		z = Qz;
		w = Qw;
	}

	GQuaternion::GQuaternion(const GVector3& Tangent, const GVector3& Binormal, const GVector3& Normal)
	{
		GMatrix4x4 TBN(Tangent.x,	Tangent.y,	Tangent.z,	0.0f,
						Binormal.x, Binormal.y, Binormal.z, 0.0f,
						Normal.x,	Normal.y,	Normal.z,	0.0f,
						0.0f,		0.0f,		0.0f,		1.0f);

		GQuaternion Q;
		GMatrix4x4::Convert(Q, TBN);

		if (Q.w < 0.0f)
		{
			x = -Q.x;
			y = -Q.y;
			z = -Q.z;
			w = -Q.w;
		}
		else
		{
			x = Q.x;
			y = Q.y;
			z = Q.z;
			w = Q.w;
		}
	
		/*
		T const bias = T(1) / ((1UL << (bits - 1)) - 1);
		if (tangent_quat.w() < bias)
		{
			T const factor = sqrt(1 - bias * bias);
			tangent_quat.x() *= factor;
			tangent_quat.y() *= factor;
			tangent_quat.z() *= factor;
			tangent_quat.w() = bias;
		}
		*/
	}

	GQuaternion::operator float* ()
	{
		return Arr;
	}

	GQuaternion::operator const float* () const
	{
		return (const float*)Arr;
	}

	void GQuaternion::Normalize()
	{
		float InvLenght = 1.0f / SqrtF(x * x + y * y + z * z + w * w);

		x *= InvLenght;
		y *= InvLenght;
		z *= InvLenght;
		w *= InvLenght;
	}

	void Normalize(OUTPUT GQuaternion& Output, const GQuaternion& Quaternion)
	{
		float LenghtSqr = Quaternion.x * Quaternion.x + Quaternion.y * Quaternion.y + 
						  Quaternion.z * Quaternion.z + Quaternion.w * Quaternion.w;
		float InvLenght = 1.0f / SqrtF(LenghtSqr);

		Output.x = Quaternion.x * InvLenght;
		Output.y = Quaternion.y * InvLenght;
		Output.z = Quaternion.z * InvLenght;
		Output.w = Quaternion.w * InvLenght;
	}

	void GQuaternion::Conjugate()
	{
		x = -x;
		y = -y;
		z = -z;
		w = w;
	}

	void GQuaternion::Conjugate(OUTPUT GQuaternion& Output, const GQuaternion& Quaternion)
	{
		Output.x = -Quaternion.x;
		Output.y = -Quaternion.y;
		Output.z = -Quaternion.z;
		Output.w =  Quaternion.w;
	}

	void GQuaternion::Multiply(OUTPUT GQuaternion& Output, const GQuaternion& QuaternionA, const GQuaternion& QuaternionB)
	{
		Output.x =  QuaternionA.x * QuaternionB.w + QuaternionA.y * QuaternionB.z - QuaternionA.z * QuaternionB.y + QuaternionA.w * QuaternionB.x;
		Output.y = -QuaternionA.x * QuaternionB.z + QuaternionA.y * QuaternionB.w + QuaternionA.z * QuaternionB.x + QuaternionA.w * QuaternionB.y;
		Output.z =  QuaternionA.x * QuaternionB.y - QuaternionA.y * QuaternionB.x + QuaternionA.z * QuaternionB.w + QuaternionA.w * QuaternionB.z;
		Output.w = -QuaternionA.x * QuaternionB.x - QuaternionA.y * QuaternionB.y - QuaternionA.z * QuaternionB.z + QuaternionA.w * QuaternionB.w;
	}

	void GQuaternion::Multiply(OUTPUT GVector3& Output, const GVector3& Vector, const GQuaternion& Quaternion)
	{
		// nVidia SDK implementation
		GVector3 UV, UUV;
		GVector3 Vec(Quaternion.x, Quaternion.y, Quaternion.z);
	
		GVector3::Cross(UV, Vec, Vector);
		GVector3::Cross(UUV, Vec, UV);
		UV *= (2.0f * Quaternion.w);
		UUV *= 2.0f;

		Output = Vector + UV + UUV;

		/*
		GQuaternion Conjugate;
		GQuaternion::Conjugate(Conjugate, Quaternion);

		GQuaternion V(Vector.x, Vector.y, Vector.z, 0.0f);

		GQuaternion Temp;
		GQuaternion::Multiply(Temp, Quaternion, V);

		GQuaternion Result;
		GQuaternion::Multiply(Result, Temp, Conjugate);

		Output.x = Result.x;
		Output.y = Result.y;
		Output.z = Result.z;
		*/
	}

	// Input Quaternion should be normalized
	void GQuaternion::Convert(OUTPUT GVector3& OutAxis, OUTPUT float& OutAngle, const GQuaternion& Quaternion)
	{
		float Cos = Quaternion.w;
		float Sin = SqrtF(1.0f - Cos * Cos);

		if ( AbsF(Sin) < 0.0005f )
		   Sin = 1.0f;

		OutAxis.x = Quaternion.x / Sin;
		OutAxis.y = Quaternion.y / Sin;
		OutAxis.z = Quaternion.z / Sin;

		OutAngle = ArcCosF(Cos) * 2.0f;
	}

	void GQuaternion::Convert(OUTPUT float& OutRotX, OUTPUT float& OutRotY, OUTPUT float& OutRotZ, const GQuaternion& Quaternion)
	{
		float xx = Quaternion.x * Quaternion.x;
		float yy = Quaternion.y * Quaternion.y;
		float zz = Quaternion.z * Quaternion.z;
		float ww = Quaternion.w * Quaternion.w;
		float Unit = xx + yy + zz + ww;
		float Test = Quaternion.w * Quaternion.x - Quaternion.y * Quaternion.z;
	
		if (Test > (0.499f * Unit))
		{
			// singularity at north pole
			OutRotX = 2.0f * ArcTan2F(Quaternion.z, Quaternion.w);
			OutRotY = G_PI_OVER_2;
			OutRotZ = 0.0f;
		}
		else if (Test < (-0.499f * Unit))
		{
			// singularity at south pole
			OutRotX = -2.0f * ArcTan2F(Quaternion.z, Quaternion.w);
			OutRotY = -G_PI_OVER_2;
			OutRotZ = 0.0f;
		}
		else
		{
			OutRotX = ArcTan2F(2.0f * (Quaternion.y * Quaternion.w + Quaternion.x * Quaternion.z), -xx - yy + zz + ww);
			OutRotY = ArcSinF(2.0f * Test / Unit);
			OutRotZ = ArcTan2F(2.0f * (Quaternion.z * Quaternion.w + Quaternion.x * Quaternion.y), -xx + yy - zz + ww);
		}
	}

	GQuaternion& GQuaternion::operator += (const GQuaternion& Quaternion)
	{
		x += Quaternion.x;
		y += Quaternion.y;
		z += Quaternion.z;
		w += Quaternion.w;

		return *this;
	}

	GQuaternion& GQuaternion::operator -= (const GQuaternion& Quaternion)
	{
		x -= Quaternion.x;
		y -= Quaternion.y;
		z -= Quaternion.z;
		w -= Quaternion.w;

		return *this;
	}

	GQuaternion& GQuaternion::operator *= (const GQuaternion& Quaternion)
	{
		GQuaternion Q(x, y, z, w);
		x =  Q.x * Quaternion.w + Q.y * Quaternion.z - Q.z * Quaternion.y + Q.w * Quaternion.x;
		y = -Q.x * Quaternion.z + Q.y * Quaternion.w + Q.z * Quaternion.x + Q.w * Quaternion.y;
		z =  Q.x * Quaternion.y - Q.y * Quaternion.x + Q.z * Quaternion.w + Q.w * Quaternion.z;
		w = -Q.x * Quaternion.x - Q.y * Quaternion.y - Q.z * Quaternion.z + Q.w * Quaternion.w;
	
		return *this;
	}

	GQuaternion& GQuaternion::operator *= (float Value)
	{
		x *= Value;
		y *= Value;
		z *= Value;
		w *= Value;

		return *this;
	}

	GQuaternion& GQuaternion::operator /= (float Value)
	{
		x /= Value;
		y /= Value;
		z /= Value;
		w /= Value;

		return *this;
	}

	GQuaternion GQuaternion::operator + () const
	{
		return *this;
	}

	GQuaternion GQuaternion::operator - () const
	{
		return GQuaternion(-x, -y, -z, -w);
	}

	GQuaternion GQuaternion::operator + (const GQuaternion& Quaternion) const
	{
		return GQuaternion(x + Quaternion.x, y + Quaternion.y,
							z + Quaternion.z, w + Quaternion.w);
	}

	GQuaternion GQuaternion::operator - (const GQuaternion& Quaternion) const
	{
		return GQuaternion(x - Quaternion.x, y - Quaternion.y,
							z - Quaternion.z, w - Quaternion.w);
	}

	GQuaternion GQuaternion::operator * (const GQuaternion& Quaternion) const
	{
		GQuaternion Q;
		Q.x =  x * Quaternion.w + y * Quaternion.z - z * Quaternion.y + w * Quaternion.x;
		Q.y = -x * Quaternion.z + y * Quaternion.w + z * Quaternion.x + w * Quaternion.y;
		Q.z =  x * Quaternion.y - y * Quaternion.x + z * Quaternion.w + w * Quaternion.z;
		Q.w = -x * Quaternion.x - y * Quaternion.y - z * Quaternion.z + w * Quaternion.w;
	
		return Q;
	}

	GQuaternion GQuaternion::operator * (float Value) const
	{
		return GQuaternion(x * Value, y * Value, z * Value, w * Value);
	}

	GQuaternion GQuaternion::operator / (float Value) const
	{
		return GQuaternion(x / Value, y / Value, z / Value, w / Value);
	}

	bool GQuaternion::operator == (const GQuaternion& Quaternion) const
	{
		return EqualF(x, Quaternion.x) && EqualF(y, Quaternion.y) &&
				EqualF(z, Quaternion.z) && EqualF(w, Quaternion.w);
	}

	bool GQuaternion::operator != (const GQuaternion& Quaternion) const
	{
		return !operator==(Quaternion);
	}

	GQuaternion operator * (float Value, const GQuaternion& Quaternion)
	{
		return GQuaternion(Value * Quaternion.x, Value * Quaternion.y, 
							Value * Quaternion.z, Value * Quaternion.w);
	}

	GQuaternion operator / (float Value, const GQuaternion& Quaternion)
	{
		return GQuaternion(Value / Quaternion.x, Value / Quaternion.y, 
							Value / Quaternion.z, Value / Quaternion.w);
	}

}